import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class wallfollow {

	float Distance;
	float diff;
	int side;
	TachoPilot p;

	class SonicThread implements Runnable {
		public void run(){
			final UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S2);
			float[] distances;
			distances = new float[10];
			while(true){

				for(int i = 9; i<0;i--){
					distances[i]=distances[i-1];
				}

				distances[0] = sonic.getRange(); 

				try{
					Thread.sleep(2);
				}catch(InterruptedException e){
				}

				// Van de bekomen 10 meetwaarden wordt dan de kleinste berekend,
				// gezien die relatief gezien het meest nauwkeurig zal zijn.
				float smallest = distances[0];
				for(int i = 1; i<distances.length;i++){
					if(distances[i]<=smallest && distances[i] > 5)
						smallest = distances[i];
				}

				Distance = smallest;

			}
		}
	}

	class Move implements Runnable {
		public void run(){
			while(true){
				p.steer(diff*side,45,true);
			}
		}
	}

	public static void main(String[] args)
	{
		// Nieuwe muurvolger starten.
		new wallfollow().go();
	}

	private void go()
	{
		// Alles initialiseren.
		p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C,true);
		Thread t = new Thread(new SonicThread());
		Thread m = new Thread(new Move());
		t.start();
		m.start();
		side =-1;
		p.setMoveSpeed(12f);
		while (true) diffCalc();

	}

	private void diffCalc(){
		float first = Distance;
		try{
			Thread.sleep(150);
		}catch(InterruptedException e){
		}
		float second = Distance;		
		diff = (second - first)*15;
		
		
	}

}

